import time
from lepi import *

def test_motor():
    # import time
    # Lepi.motor_set_speed(Lepi.MOTOR_2,0)
    # time.sleep(0.02)
    # Lepi.motor_set_enable(Lepi.MOTOR_2)
    # time.sleep(0.02)
    print(Lepi.motor_get_current_position(Lepi.MOTOR_2))
    time.sleep(0.02)
    # print(Lepi.motor_set_target_position(Lepi.MOTOR_2,720))
    # time.sleep(0.02)

    # # Lepi.motor_set_speed(Lepi.MOTOR_2,35000)

    # time.sleep(2)
    # print(Lepi.motor_get_current_position(Lepi.MOTOR_2))
    # time.sleep(0.02)

    Lepi.motor_set_enable(Lepi.MOTOR_2, 1)
    time.sleep(0.02)
    Lepi.motor_set_speed(Lepi.MOTOR_2, 200)
    time.sleep(0.02)
    print(Lepi.motor_get_speed(Lepi.MOTOR_2))
    time.sleep(0.02)
    Lepi.motor_set_speed(Lepi.MOTOR_2, 20000)
    time.sleep(0.02)
    print(Lepi.motor_get_speed(Lepi.MOTOR_2))
    Lepi.motor_set_enable(Lepi.MOTOR_2, 0)
    time.sleep(0.02)

    Lepi.motor_set_enable(Lepi.MOTOR_1, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(Lepi.MOTOR_2, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(Lepi.MOTOR_3, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(Lepi.MOTOR_4, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(port, 1)
    time.sleep(0.02)
    Lepi.motor_set_speed(Lepi.MOTOR_1, 65000)
    time.sleep(0.02)
    Lepi.motor_set_speed(Lepi.MOTOR_2, 65000)
    time.sleep(0.02)
    Lepi.motor_set_speed(Lepi.MOTOR_3, 65000)
    time.sleep(0.02)
    Lepi.motor_set_speed(Lepi.MOTOR_4, 65000)
    time.sleep(0.02)
    Lepi.motor_set_speed(port, 65000)
    time.sleep(2)
    Lepi.motor_set_enable(Lepi.MOTOR_1, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(Lepi.MOTOR_2, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(Lepi.MOTOR_3, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(Lepi.MOTOR_4, 1)
    time.sleep(0.02)
    Lepi.motor_set_enable(port, 1)


def test_sensor():
    print(Lepi.sensor_get_type(Lepi.SENSOR_2))
    time.sleep(0.02)
    print(Lepi.sensor_get_mode(Lepi.SENSOR_2))
    time.sleep(0.02)
    while True:
        print(Lepi.sensor_get_value(Lepi.SENSOR_2))
        time.sleep(0.5)


def test_servo(port):
    print(Lepi.motor_get_type(port))
    Lepi.motor_set_type(port, 1)
    time.sleep(1)
    print(Lepi.motor_get_type(port))
    Lepi.motor_set_angle(port, 0)
    time.sleep(1)
    # Lepi.motor_set_enable(port,0)
    # return
    Lepi.motor_set_angle(port, -90)
    time.sleep(1)
    Lepi.motor_set_angle(port, 0)
    time.sleep(1)
    Lepi.motor_set_angle(port, 90)
    time.sleep(1)
    Lepi.motor_set_angle(port, 0)
    # Lepi.motor_set_type(port,0)
    # Lepi.motor_set_enable(port,0)


def test_servo2(port):
    Lepi.motor_set_speed(port, 50)
    print(Lepi.motor_get_type(port))
    print(Lepi.motor_get_state(port))
    print(Lepi.motor_get_speed(port))
    time.sleep(1)
    Lepi.motor_set_state(port, 3)
    print(Lepi.motor_get_type(port))
    print(Lepi.motor_get_state(port))
    print(Lepi.motor_get_speed(port))
    time.sleep(1)

def write_speed(speed):
	print('write_speed :' ,speed)
	Lepi.motor_set_speed(Lepi.MOTOR_1,speed)
	# time.sleep(0.004)
	# time.sleep(0.02)
	Lepi.motor_set_speed(Lepi.MOTOR_2,speed)
	# time.sleep(0.004)
	# time.sleep(0.02)
	Lepi.motor_set_speed(Lepi.MOTOR_3,speed)
	# time.sleep(0.004)
	# time.sleep(0.02)
	Lepi.motor_set_speed(Lepi.MOTOR_4,speed)
	# time.sleep(0.004)
	# time.sleep(0.02)
	Lepi.motor_set_speed(Lepi.MOTOR_5,speed)	
	# time.sleep(0.004)
	# time.sleep(0.02)
def read_speed():
	print('read_speed')
	print(Lepi.motor_get_speed(Lepi.MOTOR_1))
	print(Lepi.motor_get_speed(Lepi.MOTOR_2))
	print(Lepi.motor_get_speed(Lepi.MOTOR_3))
	print(Lepi.motor_get_speed(Lepi.MOTOR_4))
	print(Lepi.motor_get_speed(Lepi.MOTOR_5))	
def read_enable():
	print('read_enable')
	print(Lepi.motor_get_enable(1))
	print(Lepi.motor_get_enable(2))
	print(Lepi.motor_get_enable(3))
	print(Lepi.motor_get_enable(4))
	print(Lepi.motor_get_enable(5))
def write_enable(enable):
	print('write_enable :' ,enable)
	Lepi.motor_set_enable(1,enable)
	Lepi.motor_set_enable(2,enable)
	Lepi.motor_set_enable(3,enable)
	Lepi.motor_set_enable(4,enable)
	Lepi.motor_set_enable(5,enable)
def test_motor():
	# Lepi.motor_set_speed(Lepi.MOTOR_1,200)
	# Lepi.motor_set_speed(Lepi.MOTOR_2,200)
	# Lepi.motor_set_speed(Lepi.MOTOR_3,200)
	# Lepi.motor_set_speed(Lepi.MOTOR_4,200)
	# Lepi.motor_set_speed(Lepi.MOTOR_5,200)	

	# time.sleep(20)
	# print(Lepi.motor_get_speed(Lepi.MOTOR_1))
	# print(Lepi.motor_get_speed(Lepi.MOTOR_2))
	# print(Lepi.motor_get_speed(Lepi.MOTOR_3))
	# print(Lepi.motor_get_speed(Lepi.MOTOR_4))
	# print(Lepi.motor_get_speed(Lepi.MOTOR_5))

	Lepi.motor_set_enable(Lepi.MOTOR_1,1)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_2,1)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_3,1)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_4,1)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_5,1)
	# time.sleep(0.02)
	print(Lepi.motor_get_enable(1))
	print(Lepi.motor_get_enable(2))
	print(Lepi.motor_get_enable(3))
	print(Lepi.motor_get_enable(4))
	print(Lepi.motor_get_enable(5))
	Lepi.motor_set_enable(Lepi.MOTOR_1,0)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_2,0)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_3,0)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_4,0)
	# time.sleep(0.02)
	Lepi.motor_set_enable(Lepi.MOTOR_5,0)	
	print(Lepi.motor_get_enable(1))
	print(Lepi.motor_get_enable(2))
	print(Lepi.motor_get_enable(3))
	print(Lepi.motor_get_enable(4))
	print(Lepi.motor_get_enable(5))
# test_motor()
def test2():
	read_speed()
	write_speed(10001)
	read_speed()
	write_speed(10002)
	read_speed()
	write_speed(10003)
	read_speed()
	write_speed(10004)
	read_speed()
	write_speed(10005)
	read_speed()	
	write_speed(10006)
	read_speed()	
	write_speed(10007)
	read_speed()	
	write_speed(10008)
	read_speed()	
	write_speed(10009)
	read_speed()	
	write_speed(10010)
	read_speed()	
	write_speed(10011)
	read_speed()	
	write_speed(10012)
	read_speed()
def test_info():
	# print(Lepi.motor_get_info(1))
	# print(Lepi.motor_get_info(2))
	# print(Lepi.motor_get_info(3))
	# print(Lepi.motor_get_info(4))
	# print(Lepi.motor_get_info(5))
	Lepi.motor_set_info(1,2,6)
	Lepi.motor_set_info(2,3,7)
	Lepi.motor_set_info(3,4,8)
	Lepi.motor_set_info(4,5,9)
	Lepi.motor_set_info(5,6,10)
	print(Lepi.motor_get_info(1))
	print(Lepi.motor_get_info(2))
	print(Lepi.motor_get_info(3))
	print(Lepi.motor_get_info(4))
	print(Lepi.motor_get_info(5))
	Lepi.motor_set_info(1,1,1001)
	Lepi.motor_set_info(2,1,1002)
	Lepi.motor_set_info(3,1,1003)
	Lepi.motor_set_info(4,1,1004)
	Lepi.motor_set_info(5,1,1005)	
	print(Lepi.motor_get_info(1))
	print(Lepi.motor_get_info(2))
	print(Lepi.motor_get_info(3))
	print(Lepi.motor_get_info(4))
	print(Lepi.motor_get_info(5))

def testRW():
        while True:
            for i in range(1,6):
              info = Lepi.motor_get_info(i)
              if info[1] != 0:
                  print('data error')
                  print(info)


if __name__ == '__main__':
	testRW()
	# test_motor()
	#write_enable(0)
	#read_enable()
	#write_speed(23)
	#read_speed()
	#write_enable(1)
	#read_speed()	
	#write_speed(233)
	#read_speed()
	#write_enable(1)
	#read_speed()
	#write_speed(2333)
	#read_speed()
	#write_speed(23333)
	#read_speed()	
	# read_speed()
	# test_info()
